//
//
#include "Ano_Imu_Data.h"
#include "ANO_Math.h"

#define G_1G_CMPSS 981
//
_imuData_st st_imuData;
_test_st test_st;
	/*IMU传感器灵敏度初始化*/
  //	ins_calibrated: 是否已经校准过加速度计的标志，1 表示已校准。
  //accRefValue: 已校准后的参考值数组，包含 X/Y/Z 三个轴的加速度原始值。
	void ImuSensitivityInit(u8 ins_calibrated,vec3_f accRefValue)
	{
		//陀螺仪灵敏度换算
		for(u8 i=0;i<3;i++)
		{
			st_imuData.gyrSensitivity[i] = 2000.0f/32767.0f;
		}
		//加速度计灵敏度换算
		if(ins_calibrated==1)
		{
			st_imuData.accSensitivity[0] = G_1G_CMPSS/accRefValue[0];
			st_imuData.accSensitivity[1] = G_1G_CMPSS/accRefValue[1];
			st_imuData.accSensitivity[2] = G_1G_CMPSS/accRefValue[2];
		}
		else
		{
			st_imuData.accSensitivity[0] = 
			st_imuData.accSensitivity[1] = 
			st_imuData.accSensitivity[2] = 0;
		}
	}
	
	
	
/*IMU传感器数据获取*/
	//主要作用是将传入的陀螺仪和加速度计原始数据拷贝到全局结构体 st_imuData 中，供后续处理使用。
void ImuDataGet(vec3_s16 gyrRaw,vec3_s16 accRaw)//获取 IMU（惯性测量单元）原始数据
{
	for(u8 i=0;i<3;i++)
	{
		//st_imuData：全局变量
		st_imuData.f_gyrRaw[i] = gyrRaw[i];
		st_imuData.f_accRaw[i] = accRaw[i];
		//		
	}
}

/*IMU温度获取*/
void ImuTemperatureGet(float f_temperature)
{
	st_imuData.f_temperature = f_temperature;
}


/*IMU数据计算处理*/
//它完成了从原始传感器数据到物理单位、坐标变换、角度转弧度等关键步骤
//                是否已经完成校准  陀螺仪偏移量      加速度计偏移量  坐标系转换矩阵
void ImuDataCalcu(u8 ins_calibrated,vec3_f gyrOffset,vec3_f accOffset,float IEM[3][3])
{
	if(ins_calibrated==1)//ins_calibrated：校准变量，1为已经校准
	{
		//
		st_imuData.data_sta = 1; //表示当前 IMU 数据有效、可用。
		//转换单位
		for(u8 i=0;i<3;i++)
		{
			// 数据校准 + 单位换算（核心部分）
			//对原始数据进行 零点校准（减去偏移量）
      //使用灵敏度系数将其 转换单位
      //陀螺仪：原始值 → deg/s
      //加速度计：原始值 → cm/s²（或 m/s² 或 g）
			st_imuData.f_gyr_dps[i]   = (st_imuData.f_gyrRaw[i] - gyrOffset[i])*st_imuData.gyrSensitivity[i];
			st_imuData.f_acc_cmpss[i] = (st_imuData.f_accRaw[i] - accOffset[i])*st_imuData.accSensitivity[i];
			//		
		}
		
		//转ANO坐标系
		//将传感器数据从其默认坐标系（如 MPU6050 的 X/Y/Z）
		//转换为飞控使用的标准坐标系（如 ANO、ArduPilot 等定义的坐标系）。
		Vec3f_Mul_MatrixT(st_imuData.f_gyr_dps,IEM,st_imuData.f_gyr_dps_nb);
		Vec3f_Mul_MatrixT(st_imuData.f_acc_cmpss,IEM,st_imuData.f_acc_cmpss_nb);
		
		//角度转弧度
		for(u8 i=0;i<3;i++)
		{
			//角速度从 deg/s 转换为 rad/s，因为后续的姿态积分或滤波算法通常使用弧度制。
			st_imuData.f_gyr_radps_nb[i] = RAD_PER_DEG *st_imuData.f_gyr_dps_nb[i];
		}
	}
	else//在未校准状态下，仅设置状态标志为不可用，并将原始值赋给输出字段，方便调试或提示用户需要校准
	{
		//
		st_imuData.data_sta = 0;
		//
		for(u8 i=0;i<3;i++)
		{
			//未校准，输出原始值
			st_imuData.f_gyr_dps_nb[i]   = st_imuData.f_gyrRaw[i];
			st_imuData.f_acc_cmpss_nb[i] = st_imuData.f_accRaw[i];
		}	
	}
}




